#include "sensor_impedance.h"
#include "sensor_welectrode.h"

#define BUFFER_SIZE                 5
#define BUFFER_FILTER_LENGTH        4
#define STABLE_TIME             (   6 * SECONDS_PER_HOUR )
#define STABLE_TIME_AFTER_CAL   (   1 * SECONDS_PER_HOUR )
#define SENS_CHANGE_CAL_TIME    (   6 * SECONDS_PER_HOUR )

#define SENS_FACTOR_DEFAULT         ( 100 * PERCISION / 100 )
#define SENS_FACTOR_GRADUAL_RATE    (   5 * PERCISION / 100 )
#define REAL_CHANGE_LEVEL_1         ( 5 * PERCISION )
#define IMAG_CHANGE_LEVEL_1         ( 0 * PERCISION )


static uint64 (*m_pf_ReadImpedance)(uint);
static fixed32 m_f32_currentReal[FREQUENCY_COUNT] = {0};
static fixed32 m_f32_currentImag[FREQUENCY_COUNT] = {0};
static fixed32 m_f32_realBufData[FREQUENCY_COUNT][BUFFER_SIZE] = {0};
static fixed32 m_f32_imagBufData[FREQUENCY_COUNT][BUFFER_SIZE] = {0};
static cycle_buffer m_t_realBuf[FREQUENCY_COUNT] = {0};
static cycle_buffer m_t_imagBuf[FREQUENCY_COUNT] = {0};
static int32 m_i32_valid[BUFFER_SIZE] = {0};
static filter_window t_valid = { m_i32_valid, BUFFER_FILTER_LENGTH, 0 };

static fixed32 m_f32_refReal = 0;
static fixed32 m_f32_refImag = 0;
static uint32 m_u32_calTime = 0;
static fixed32 m_f32_prevSensFactor = SENS_FACTOR_DEFAULT;
static fixed32 m_f32_sensFactor = SENS_FACTOR_DEFAULT;
static fixed32 m_f32_prevSensOffset = 0;
static fixed32 m_f32_sensOffset = 0;
static logical m_b_sensChanged = L_FALSE;

static logical Impedance_IsRealInRange(fixed32 f32_real);
static logical Impedance_IsImagInRange(fixed32 f32_imag);
static logical Impedance_CheckSensChanged( uint32 u32_time );
#ifdef ENABLE_IMP_AUTOCAL
static logical Impedance_CalculateSens( uint32 u32_time );
#endif
#ifdef ENABLE_IMP_AUTOCAL_PROG
static logical Impedance_CalculateSens_Prog( uint32 u32_time );
#endif

void Impedance_Initialize( uint32 u32_calTime, comp64 c64_refImp,
                           uint64 (*pf_ReadImpedance)(uint) )
{
    Impedance_Reset();
    m_u32_calTime = u32_calTime;
    m_f32_refReal = Real(c64_refImp);
    m_f32_refImag = Imag(c64_refImp);
    m_pf_ReadImpedance = pf_ReadImpedance;
}

void Impedance_Reset()
{
    uint i;
    for ( i=0; i<FREQUENCY_COUNT; i++ )
    {
        m_f32_currentReal[i] = 0;
        m_f32_currentImag[i] = 0;
        Buffer_Init( &m_t_realBuf[i], &m_f32_realBufData[i][0], BUFFER_SIZE );
        Buffer_Init( &m_t_imagBuf[i], &m_f32_imagBufData[i][0], BUFFER_SIZE );
    }

    m_u32_calTime = 0;
    m_f32_refReal = DP.REF_REAL_DEFAULT;
    m_f32_refImag = DP.REF_IMAG_DEFAULT;
    m_f32_prevSensFactor = SENS_FACTOR_DEFAULT;
    m_f32_sensFactor = SENS_FACTOR_DEFAULT;
    m_f32_prevSensOffset = 0;
    m_f32_sensOffset = 0;
    m_b_sensChanged = L_FALSE;
}

logical Impedance_Update( uint32 u32_time )
{
    comp64 c64_value;

    uint i;
    for ( i=0; i<FREQUENCY_COUNT; i++ )
    {
        c64_value = m_pf_ReadImpedance(i);
        m_f32_currentReal[i] =  Real(c64_value);
        m_f32_currentImag[i] = -Imag(c64_value);
        Buffer_Push( &m_t_realBuf[i], m_f32_currentReal[i] );
        Buffer_Push( &m_t_imagBuf[i], m_f32_currentImag[i] );
    }
    for ( i=0; i<BUFFER_FILTER_LENGTH; i++ )
        m_i32_valid[BUFFER_FILTER_LENGTH-i] = m_i32_valid[BUFFER_FILTER_LENGTH-i-1];
    m_i32_valid[0] = ( WE_IsRefValid() &&
                       Impedance_IsRealInRange(m_f32_currentReal[0]) &&
                       Impedance_IsImagInRange(m_f32_currentImag[0]) )
                     ? 1
                     : 0;

#if defined ENABLE_IMP_AUTOCAL_PROG
    Impedance_CalculateSens_Prog(u32_time);
#endif

    if ( WE_IsRefValid() && Impedance_Valid() && Impedance_Assert() )
    {
#if defined ENABLE_IMP_AUTOCAL
        Impedance_CalculateSens(u32_time);
#endif
        if ( m_u32_calTime > 0 && u32_time < m_u32_calTime + STABLE_TIME_AFTER_CAL )
        {
            if ( u32_time > STABLE_TIME && m_t_realBuf[0].u32_count >= BUFFER_FILTER_LENGTH )
            {
                m_f32_refReal = Buffer_Mean( &m_t_realBuf[0], BUFFER_FILTER_LENGTH);
                m_f32_refImag = Buffer_Mean( &m_t_imagBuf[0], BUFFER_FILTER_LENGTH);
                return L_TRUE;
            }
        }
        else
        {
            Impedance_CheckSensChanged(u32_time);
        }
    }

    return L_FALSE;
}

fixed32 Impedance_GetSensFactor( logical t_gradual )
{
    fixed32 f32_diff = m_f32_sensFactor - m_f32_prevSensFactor;
    if (t_gradual)
        f32_diff = CLIP(f32_diff, SENS_FACTOR_GRADUAL_RATE, -SENS_FACTOR_GRADUAL_RATE);
    m_f32_prevSensFactor += f32_diff;

    return m_f32_prevSensFactor;
}

fixed32 Impedance_GetSensOffset( logical t_gradual )
{
    fixed32 f32_diff = m_f32_sensOffset - m_f32_prevSensOffset;
    if (t_gradual)
        f32_diff = CLIP(f32_diff, SENS_FACTOR_GRADUAL_RATE, -SENS_FACTOR_GRADUAL_RATE);
    m_f32_prevSensOffset += f32_diff;

    return m_f32_prevSensOffset;
}

logical Impedance_SensChanged(void) { return m_b_sensChanged; }

#ifdef ENABLE_IMP_AUTOCAL
static logical Impedance_CalculateSens( uint32 u32_time )
{
    if (m_i32_valid[0] + m_i32_valid[1] + m_i32_valid[2] < 3)
        return L_FALSE;

    fixed32 f32_hour, f32_real, f32_imag;

    if ( m_t_realBuf[0].u32_count == 0 )
        return L_FALSE;

    f32_real = Buffer_Filter( &m_t_realBuf[0], &t_valid);
    f32_imag = Buffer_Filter( &m_t_imagBuf[0], &t_valid);
    if ( Impedance_IsRealInRange(f32_real) && Impedance_IsImagInRange(f32_imag) )
    {
        f32_hour = u32_time / SECONDS_PER_MINUTE * PERCISION / MINUTES_PER_HOUR;
        if (f32_hour > DP.REF_REAL_CHANGE_START_HOUR)
            f32_hour -= DP.REF_REAL_CHANGE_START_HOUR;
        else
            f32_hour = 0;
        f32_real = f32_real - DP.REF_REAL_DEFAULT - F_MUL( DP.REF_REAL_CHANGE_PER_HOUR, f32_hour );
        f32_imag = f32_imag - DP.REF_IMAG_DEFAULT;
        f32_imag = F_MAX(f32_imag, 0);
        m_f32_sensOffset = F_MUL(f32_imag, DP.REF_IMAG_OFFSET);
        m_f32_sensFactor = PERCISION + F_MUL(f32_real, DP.REF_REAL_FACTOR) / 100 + F_MUL(f32_imag, DP.REF_IMAG_FACTOR) / 100;
        f32_real = CLIP(f32_real, REAL_CHANGE_LEVEL_1, -REAL_CHANGE_LEVEL_1);
        f32_imag = CLIP(f32_imag, IMAG_CHANGE_LEVEL_1, -IMAG_CHANGE_LEVEL_1);
        m_f32_sensFactor -= F_MUL(f32_real, DP.REF_REAL_FACTOR) / 200 + F_MUL(f32_imag, DP.REF_IMAG_FACTOR) / 200;
        m_f32_sensFactor = CLIP( m_f32_sensFactor, DP.SENS_FACTOR_UPPER, DP.SENS_FACTOR_LOWER );
    }

    return L_TRUE;
}
#endif
#ifdef ENABLE_IMP_AUTOCAL_PROG
static logical Impedance_CalculateSens_Prog( uint32 u32_time )
{
    fixed32 f32_offset = 0;

    f32_offset = u32_time / SECONDS_PER_MINUTE * PERCISION;
    f32_offset = F_MUL( f32_offset, DP.REF_REAL_FACTOR );
    f32_offset = S_DIV( f32_offset, MINUTES_PER_HOUR );
    f32_offset = F_DIV( f32_offset, DP.OFFSET_PERIOD );
    f32_offset = DP.REF_REAL_FACTOR - f32_offset;
    f32_offset = F_MAX( f32_offset, 0 );

    m_f32_sensFactor = SENS_FACTOR_DEFAULT + f32_offset;

    return L_TRUE;
}
#endif

static logical Impedance_CheckSensChanged( uint32 u32_time )
{
    fixed32 f32_real, f32_imag;

    if ( m_t_realBuf[0].u32_count >= 2 && u32_time > m_u32_calTime + SENS_CHANGE_CAL_TIME)
    {
        f32_real = Buffer_Get( &m_t_realBuf[0], 1 );
        f32_imag = Buffer_Get( &m_t_imagBuf[0], 1 );
        m_b_sensChanged =
                ( ABS(m_f32_currentReal[0]-m_f32_refReal) > DP.REF_REAL_SENS_CHANGED &&
                  ABS(  f32_real          -m_f32_refReal) > DP.REF_REAL_SENS_CHANGED ) ||
                ( ABS(m_f32_currentImag[0]-m_f32_refImag) > DP.REF_IMAG_SENS_CHANGED &&
                  ABS(  f32_imag          -m_f32_refImag) > DP.REF_IMAG_SENS_CHANGED ) ;
    }
    return m_b_sensChanged;
}

fixed32 Impedance_GetRefReal(void)
{
    return m_f32_refReal;
}

fixed32 Impedance_GetRefImag(void)
{
    return m_f32_refImag;
}

void Impedance_SensReset( uint32 u32_time )
{
    if ( u32_time < STABLE_TIME )
        return;

    m_u32_calTime = u32_time;
    m_f32_refReal = Buffer_Mean( &m_t_realBuf[0], BUFFER_FILTER_LENGTH);
    m_b_sensChanged = L_FALSE;
}

static logical Impedance_IsRealInRange(fixed32 f32_real)
{
    return f32_real >  DP.REF_REAL_LOWER &&
           f32_real <= DP.REF_REAL_UPPER;
}

static logical Impedance_IsImagInRange(fixed32 f32_imag)
{
    return f32_imag >  DP.REF_IMAG_LOWER &&
           f32_imag <= DP.REF_IMAG_UPPER;
}

logical Impedance_Valid(void)
{
    return m_f32_currentReal[0] < 326*PERCISION;
}

logical Impedance_Assert(void)
{
    if ( m_t_realBuf[0].u32_count < 2 )
        return L_TRUE;

    return !Impedance_IsRealInRange( m_f32_currentReal[0] ) ||
            Impedance_IsImagInRange( m_f32_currentImag[0] ) ||
           !Impedance_IsRealInRange( Buffer_Get(&m_t_realBuf[0], 1 )) ||
            Impedance_IsImagInRange( Buffer_Get(&m_t_imagBuf[0], 1) );
}

logical Impedance_IsCurrentRealInRange()
{
    return Impedance_IsRealInRange( m_f32_currentReal[0] );
}
